#coding:utf-8
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np
import time

# from parking import select_path
# from parking import constants as cons
import select_path
import constants as cons
import copy

import utils
import utils_list
import file_utils
import move_out_policy
import move_in_policy
import utils_move_plot as ump
import circle_move

# global init_position # 平板车的初始位置

def move_out_step4(ax, free_point, target, trace, occupied):
    '''
    <class 'list'>:
         [
         [[[1, 2], [1, 1]], [[1, 1], [1, 2], [2, 2]]],
         [[[2, 2], [1, 2]], [[1, 2], [2, 2], [2, 1]]],
         [[[2, 1], [2, 2]], [[2, 2], [2, 1], [1, 1]]],
         [[2, 1], [2, 1]]
         ]
    :param free_point:
    :param target:
    :return:
    '''
    ump.plot_car_park_occupied(occupied, ax)
    ump.plot_per_car_park_occupied(target, ax, color=cons.COLOR_TARGET)  # 标识 target 的位置

    pairs, free_point, target = circle_move.construct_trace_step4(free_point, target, trace)

    stage_3 = pairs[-1]
    pairs = pairs[0:-1]
    for pair in pairs:
        stage_1 = pair[0]
        stage_2 = pair[1]

        move_stage_one(ax, stage_1, cons.occupied)
        ump.move_pingbancar(stage_2, ax, reset_car=False)

    # 将target沿着轨迹线往外移出一位
    move_stage_one(ax, stage_3, cons.target)

    return free_point, target

def move_out_step5(ax, free_point, target, trace, occupied):
    '''
    pairs
    pair <class 'list'>: [[[2, 1], [1, 1], [1, 2]],     [[1, 2], [1, 1]]]
    pair <class 'list'>: [[[1, 1], [1, 2], [2, 2]],     [[2, 2], [1, 2]]]
    pair <class 'list'>: [[[1, 2], [2, 2], [3, 2]],     [[3, 2], [2, 2]]]
    pair <class 'list'>: [[[2, 2], [3, 2], [3, 1]],     [[3, 1], [3, 2]]]
    pair <class 'list'>: [[[3, 2], [3, 1], [2, 1]],     [[2, 1], [3, 1]]]
    :param free_point:
    :param target:
    :param trace:
    :return:
    '''
    ump.plot_car_park_occupied(occupied, ax)
    ump.plot_per_car_park_occupied(target, ax, color=cons.COLOR_TARGET)  # 标识 target 的位置

    pairs, free_point, target = circle_move.construct_trace_step5(free_point, target)
    free_point_temp = copy.copy(free_point)
    target_temp = copy.copy(target)

    i = 0
    for pair in pairs:
        # [[2, 1], [1, 1], [1, 2]]
        # [[1, 2], [1, 1]]
        stage_1 = pair[0]
        stage_2 = pair[1]
        ump.move_pingbancar(stage_1, ax, reset_car=False)

        i = i + 1
        if i == len(pairs):
            move_stage_one(ax, stage_2, cons.target)
        else:
            move_stage_one(ax, stage_2, cons.occupied)

    return free_point_temp, target_temp

def move_stage_one(ax, pair, occupied_target=cons.occupied):
    '''
    移动一次循环
    :param pair: 移动轨迹
    :param occupied_target: 标识移动的是障碍物还是target
    :return:
    '''
    # 最后将 target 移出
    ump.plot_per_car_park_occupied(pair[0], ax, color=cons.COLOR_FLATCAR)  # 表示被移动的车辆
    ump.move_pingbancar(pair, ax, reset_car=False)
    ump.plot_per_car_park_occupied(pair[0], ax, color=cons.COLOR_PARK)  # 标识无车的位置
    if occupied_target == cons.occupied:
        ump.plot_per_car_park_occupied(pair[-1], ax, color=cons.COLORT_OCCUPIED)  # 标识被占有的位置
    elif occupied_target == cons.target:
        ump.plot_per_car_park_occupied(pair[-1], ax, color=cons.COLOR_TARGET)  # 标识 target 的位置


def move_out_subnode(ax, trace, occ, target):
    '''
    沿着轨迹线，将所有的障碍往外移出一位
    :param trace:
    :param occ:
    :return: 最新的 free_point, target
    '''
    # 构造移出方式
    # pair_stage_1 = [[3, 1], [4, 1]]               # 移动车辆
    # pair_stage_2 = [[4, 1], [3, 1], [2, 1]]       # 移动平板车
    # pair = [pair_stage_1, pair_stage_2]
    # pairs = [pair, pair, pair]

    ump.plot_car_park(ax)
    ump.plot_car_park_occupied(occ, ax)
    ump.plot_per_car_park_occupied(target, ax, color=cons.COLOR_TARGET)  # 标识 target 的位置
    # 构造此处移动的轨迹
    pairs = circle_move.construct_trace(trace, occ)
    # stage_3 = pairs[-1]
    # pairs = pairs[0:-1]
    for pair in pairs:
        stage_1 = pair[0]
        stage_2 = pair[1]

        # 移动车辆
        move_stage_one(ax, stage_1, cons.occupied)
        ump.move_pingbancar(stage_2, ax, reset_car=False)

    # ump.plot_per_car_park_occupied(stage_3[0], ax, color=cons.COLOR_FLATCAR)  # 表示被移动的车辆
    # ump.move_pingbancar(stage_3, ax, reset_car=False)
    # ump.plot_per_car_park_occupied(stage_3[0], ax, color=cons.COLOR_PARK)  # 标识无车的位置
    # ump.plot_per_car_park_occupied(stage_3[-1], ax, color=cons.COLOR_TARGET)  # 标识被占有的位置
    free_point = stage_1[0]
    target = stage_2[-1]
    #
    return free_point, target

def move_out_one_step(ax, trace_, occupied, in_out):
    ump.plot_car_park(ax)
    ump.plot_car_park_occupied(occupied, ax)
    all_trace = file_utils.read_file(cons.trace_file)
    target, target_next, all_obstacle, first_free, first_obstacle, out_pair= \
        circle_move.get_point_from_trace(trace=trace_, occ=occupied)
    # 将车移动到 first obstracl
    car_to_car = utils.get_trace(first_obstacle, all_trace)
    # 将车循环移出
    pairs, traces = circle_move.construct_trace_one_step(target, first_free, all_obstacle, out_pair, occupied)

    # 移车
    ump.move_pingbancar(car_to_car, ax, reset_car=False)
    # 计算 car_to_car 开销
    # cost_car_to_car_distance, cost_car_to_car_time = utils.compute_distance_score(car_to_car, use_important_level=cons.use_important_level)
    # cost_car_to_car_energy = utils.computer_energy_score(car_to_car)
    # cost_car_to_car = cost_car_to_car_distance + cost_car_to_car_time + cost_car_to_car_energy

    move_flat_car_only = list()
    move_flat_car_with_car = list()

    move_flat_car_only.append(car_to_car)

    for trace in traces:
        stage_1 = trace[0]
        stage_2 = trace[1]

        # 移动车辆
        if len(stage_1) != 0:
            move_stage_one(ax, stage_1, cons.occupied)
            move_flat_car_with_car.append(stage_1)
        if len(stage_2) != 0:
            ump.move_pingbancar(stage_2, ax, reset_car=False)
            move_flat_car_only.append(stage_2)

    return move_flat_car_with_car, move_flat_car_only

def park_position(ax, pair, pair_occupied, in_out, reset_car=False):
    '''
    :param ax:
    :param pair: 轨迹坐标,成对出现
    :param pair_occupied: 已经被占的位置
    :param in_out: in: True, out:False
    :param reset_car:
    :return:
    '''
    # plt.pause(4)
    # 画出所有停车位
    ump.plot_car_park(ax)
    if in_out == cons.IN: # in
        car_to_car = pair[0]
        # 检查路径上是否有障碍物或者损坏情况
        _, points_obstacle = utils.check_obstacle(pair, occ=pair_occupied)
        if len(points_obstacle) > 0: return False
        ump.plot_car_park_occupied(pair_occupied, ax)
        # 移动平板车
        ump.move_pingbancar(car_to_car, ax, reset_car)
        # 更新 occupied
        pair_occupied = utils.get_pair_occupied(car_to_car[-1], True, pair_occupied)
        # 将车送到位置之后，移动平板车到一个最优位置
        reset_pair = ump.reset_flatcar(car_to_car[-1], pair_occupied, ax)
        return pair_occupied

    elif in_out == cons.OUT: # out
        # 添加移除小车的轨迹
        print("正在移除小车...")
        # 正产移动车
        car_to_car, car_to_out = pair[0], pair[1]
        ump.plot_car_park_occupied(pair_occupied, ax)
        ump.move_pingbancar(car_to_car, ax, reset_car=True)
        utils.get_pair_occupied(car_to_out[0], False, pair_occupied)
        ump.plot_per_car_park_occupied(car_to_out[0], ax, color= cons.COLOR_FLATCAR) # 标识被移动的停车位
        ump.move_pingbancar(car_to_out, ax, reset_car)
        ump.plot_per_car_park_occupied(car_to_out[0], ax, color= cons.COLOR_PARK) #设置被移动之后的停车位为空位

        # 将车送到位置之后，移动平板车到一个最优位置
        reset_pair = ump.reset_flatcar(car_to_out[-1], pair_occupied, ax)

        pass
    elif in_out == cons.OUT_POLICY_ONE:
        # 先移动其他车，再移动 target
        flat_to_car, car_to_free, free_to_target, target_to_out = pair
        # plt.cla()
        ump.plot_car_park_occupied(pair_occupied, ax)

        ump.move_pingbancar(flat_to_car, ax, reset_car=True)

        # 重新调整 occupied
        # utils.plot_car_park_occupied(pair_occupied, ax, park_space, alpha)

        ump.plot_per_car_park_occupied(flat_to_car[-1], ax, color=cons.COLOR_FLATCAR) # 表示被移动的车辆
        ump.move_pingbancar(car_to_free, ax, reset_car=False)
        ump.plot_per_car_park_occupied(flat_to_car[-1], ax, color=cons.COLOR_PARK) # 标识无车的位置

        ump.plot_per_car_park_occupied(car_to_free[-1], ax, color=cons.COLORT_OCCUPIED) # 标识被占有的位置
        # 更新 occupied
        utils.get_pair_occupied(flat_to_car[-1], False, occupied) # 移除原先的
        utils.get_pair_occupied(car_to_free[-1], True, occupied) # 增加现有的

        ump.move_pingbancar(free_to_target, ax, reset_car=True)

        ump.plot_per_car_park_occupied(free_to_target[-1], ax, color=cons.COLOR_FLATCAR) # 表示被移动的车辆
        ump.move_pingbancar(target_to_out, ax, reset_car=False)
        ump.plot_per_car_park_occupied(free_to_target[-1], ax, color=cons.COLOR_PARK) # 标识无车的位置

        # 更新 occupied
        utils.get_pair_occupied(free_to_target[-1], False, occupied)

        # 重置平板车的位置，这个需要重新考虑
        # reset_pair = ump.reset_flatcar(free_to_target[-1], occupied, pair_occupied, ax)
        pass

    # 将平板车最终的位置标记为占有 % 为什么显示不出来？？
    # pair_occupied.append(pair[-1])
    # plot_per_car_park_occupied(pair[-1], ax, park_space, alpha)
    # print("pair_occupied %s" % pair_occupied)
    # utils.plot_car_park_occupied(pair_occupied, ax, park_space, alpha)

    # plt.show()

def get_out_car_trace(out, all_trace):
    # all_trace = file_utils.read_file(cons.trace_file)
    out_trace = utils.get_trace(out, all_trace)
    if not out_trace:
        print('there is no such a trace!')
        return False
    return out_trace

def get_in_car_trace(init, in_, occupied, use_important_level=False):
    '''
    得到平板车轨迹图, 并计算开销
    根据目的位置，通过算法得到通往目的地的轨迹图，形成car_pair

    分为两个部分：叶子节点的路径选择，子根节点的路径选择

    :param init: 初始化的位置
    :param in_: 要送入的位置
    :param occupied: 被占用的位置
    :return:
    '''
    stage = []

    init_position_copied = utils_list.get_copied(init)
    in_copied = utils_list.get_copied(in_)
    occ = utils_list.get_copied_list(occupied)

    if in_ in occupied:
        return False

    # 叶子节点的路径选择
    # 限制寻路路径
    tmp_occupied = utils.get_tmp_occupied(in_copied)
    # 直接得到路径, 不存在障碍
    car_to_car = select_path.select_path(init, in_copied, tmp_occupied, state='in')

    # 计算开销
    # if policy == cons.in_queue:
    #     in_cost = move_in_policy.get_cost(car_to_car, use_important_level=use_important_level)
    #     print("totoal in cost: %s" % in_cost)
    #     pass
    # elif policy == cons.in_handcraft:
    #     in_cost = move_in_policy.get_cost(car_to_car, use_important_level=use_important_level)
    #     print("totoal in cost: %s" % in_cost)
    #     pass
    # elif policy == cons.in_random:
    #     in_cost = move_in_policy.get_cost(car_to_car, use_important_level=use_important_level)
    #     print("totoal in cost: %s" % in_cost)
    in_cost = move_in_policy.get_cost(car_to_car, use_important_level=use_important_level)

    if not car_to_car: # 当一次进不到 target 位置的情况，不过不允许存在这种情况
        policy_stage = move_in_policy.policy_one(init_position_copied, in_copied, occ)
        # policy_stage = move_in_policy.policy_two(init_position_copied, in_copied, occ)
        stage.append(policy_stage)
    else:
        stage.append(car_to_car)

    # 保存轨迹
    # utils.save_trace(in_copied, car_to_car)

    return stage

def get_remove_car_trace(out, occupied, init_position, all_trace, policy=cons.out_policy_two):
    '''
    得到移除平板车的轨迹图
    :param out:
    :return:
    '''
    stage = []
    # 以下操作不影响原始数据
    init_position_copied = utils_list.get_copied(init_position)
    out_copied = utils_list.get_copied(out)
    occ = utils_list.get_copied_list(occupied)

    # 从小车的位置到要移除的车的位置的轨迹
    car_to_car = select_path.select_path(init_position_copied, out_copied, occ, state='remove')

    if policy == cons.out_policy_one:
        pass
    elif policy == cons.out_policy_two:
        # 根据停放车辆保存的路径来移出小车
        stage = move_out_policy.policy_two(out_copied, occ, all_trace=all_trace)
        if not stage:  # 没有该路径
            print("There is no trace for point %s" % out_copied)
            return False
        return stage

    if not car_to_car:
        # 移动的完整路径
        policy_trace = move_out_policy.policy_one(init_position_copied, out_copied, occ)
        # policy_trace = move_out_policy.policy_two(out, init_position_copied, out_copied, occ)
        stage.append(policy_trace)
        return stage[0]
    else:
        stage.append(car_to_car)
        # 从要移除的车的位置到出口的轨迹
        # out_position = [4, 9]
        min_score, out_position, out_trace = utils.get_min_cost_position(out_copied, cons.pos_out, occ)
        # remove_car_pair = select_path.select_path(out, out_position, occ, state='remove')
        stage.append(out_trace)
        return stage

def get_remove_by_trace(ax, out, occupied, init_position, all_trace, policy=cons.out_policy_two):
    '''
    根据进车路径得到移车路径
    :param out:
    :param occupied:
    :param init_position:
    :param all_trace:
    :param policy:
    :return:
    '''
    stage = []
    # 以下操作不影响原始数据
    init_position_copied = utils_list.get_copied(init_position)
    out_copied = utils_list.get_copied(out)
    occ = utils_list.get_copied_list(occupied)

    # 从小车的位置到要移除的车的位置的轨迹
    car_to_car = select_path.select_path(init_position_copied, out_copied, occ, state='remove')
    # or
    car_to_car = utils.get_trace(out, all_trace)
    trace = copy.copy(car_to_car)

    # 检查路径上是否存在障碍
    points, points_obstacle = utils.check_obstacle(trace, occupied)

    if len(points_obstacle) != 1:          # 不存在通路
        if policy == cons.out_policy_one:
            free_point, target = move_out_subnode(ax, trace, occupied,
                                                  out_copied)  # 移动枝干上的障碍物，下步将小车送到枝干上
            free_point, target = move_out_step4(ax, free_point, target, trace,
                                                occ)  # 构建四个位置的循环，将 target 移出到 枝干上

            # 检查路径上是否存在障碍
            while True:
                points, points_obstacle = utils.check_obstacle(trace, occ, target)
                if points_obstacle:  # 仍然存在障碍点
                    free_point, target = move_out_step5(ax, free_point, target, trace,
                                                        occ)  # 构建六个位置的循环，在枝干上循环移动平板车
                else:
                    break
            # 按照通路移出小车  有待修改
            ump.move_pingbancar(points, ax, reset_car=True)
            # occupied = utils.get_pair_occupied(False, in_out, occupied)

        elif policy == cons.out_policy_two:
            # 根据停放车辆保存的路径来移出小车
            # stage = move_out_policy.policy_two(out_copied, occ, all_trace=all_trace)
            # if not stage:  # 没有该路径
            #     print("There is no trace for point %s" % out_copied)
            #     return False
            # return stage
            # policy two: 构建一个大循环，一步到位将小车移动出
            move_flat_car_with_car, move_flat_car_only = move_out_one_step(ax, trace, occ, out_copied)
            # occupied = utils.get_pair_occupied(False, in_out, occupied)
            out_cost = move_out_policy.get_cost([move_flat_car_with_car, move_flat_car_only], use_important_level=cons.use_important_level)

    else:
        stage.append(car_to_car)
        reset_trace = copy.copy(car_to_car)
        reset_trace.reverse()
        stage.append(reset_trace)
        if len(stage) == 2:
            park_position(ax, stage, occ, in_out=cons.OUT)
        elif len(stage) == 4:
            park_position(ax, stage, occ, in_out=cons.OUT_POLICY_ONE)
        else:
            print('No Trace')
        # return stage
        out_cost = move_in_policy.get_cost(car_to_car)
    print('remove done.')

if __name__ == '__main__':
    # 作为全局变量，控制车位占用情况
    # pair_occupied = []
    init_position = cons.pos_out[0] # 初始位置
    # occupied = []
    # 画布属性
    fig, ax = plt.subplots()

    ax.axis([0, cons.axis_num * cons.axis_alpha, 0, cons.axis_num * cons.axis_alpha])
    ax.set_title(cons.axis_title)
    ax.set_xlabel(cons.axis_xlable)
    ax.set_ylabel(cons.axis_ylable)

    np.random.seed(2018)

    # 初始化所有参数信息
    # global fig_dicts, fig_costs
    # global fig_queue_in, fig_queue_out

    fig_dicts, fig_costs = utils.init_figure()
    fig_queue_in = utils.get_cost_queue_in(fig_dicts, fig_costs)

    occupied = [[1, 9], [2, 9], [3, 9], [4, 9], [6, 9], [7, 9], [8, 9],
                [1, 8], [2, 8], [3, 8], [4, 8], [6, 8], [7, 8], [8, 8], [2, 7]]

    # occupied = [[0, 3], [1, 3], [2, 3], [3, 3], [4, 3],
    #             [0, 2], [1, 2], [2, 2], [3, 2], [4, 2],
    #             [0, 1], [1, 1], [2, 1], [3, 1],
    #             [0, 0], [1, 0], [2, 0], [3, 0], [4, 0]]
    # 2_7:
    # <class 'list'>: [[[5, 0], [4, 0], [3, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [2, 5], [2, 6], [2, 7]]]

    all_trace = file_utils.read_file(cons.trace_file)
    # all_trace = {'2_8': [[[2, 8], [2, 7], [2, 6], [2, 5], [2, 4], [2, 3], [2, 2], [2, 1]], [[2, 1], [2, 2], [2, 3]]]}
    # all_trace = {'2_7': [[[5, 0], [4, 0], [3, 0], [2, 0], [2, 1], [2, 2], [2, 3], [2, 4], [2, 5], [2, 6], [2, 7]]]}

    # trace 的形式应该是 all_trace: {'point': [[pos], [pos], [pos], [pos]]}
    all_trace = {'1_2': [[[5, 0], [5, 1], [4, 1], [3, 1], [2, 1], [1, 1], [1, 2]]]}

    while(True):
        try:
            # sleep_seconds = np.random.randint(cons.time_start, cons.time_end, 1)[0]
            sleep_seconds = 0.1
            time.sleep(cons.sleep_seconds)
            # flag, in_out = move_in_policy.get_random_position()  # 每隔一段时间，随机进、出车辆

            in_out = move_in_policy.get_queue_position(fig_queue_in, occupied)

            # flag, in_out = move_in_policy.get_handcraft_position() # 手动输入

            # in_out = [1, 2]
            flag = cons.IN
            print("%s is %s" % (flag, in_out))
        except ValueError as e:
            print("请按照规则输入")
            continue

        if flag == cons.IN:
            print("it is in")
            pair = get_in_car_trace(init_position, in_out, occupied)
            if not pair:
                print("there is a car at the position (%s, %s) " % (in_out[0], in_out[1]))
                continue
            park_position(ax, pair, occupied, in_out=cons.IN)
            # utils.plot_car_park_occupied(occupied, ax) # 画出当前被占有的停车位
            utils_list.print_list(occupied)
        elif flag == cons.OUT:
            # park_position(ax, None, occupied)
            if not utils_list.out_pair(in_out, occupied):
                continue
            utils_list.print_list(occupied)
            # 根据 target 获取 整个 trace
            trace = get_out_car_trace(in_out)

            ump.plot_car_park_occupied(occupied, ax)
            ump.plot_car_park(ax)
            # =====================================================================
            # policy two: 构建一个大循环，一步到位将小车移动出

            move_out_one_step(ax, trace, occupied, in_out)

            # =============================== policy two===========================
            # =====================================================================


            # =====================================================================
            # =============================== policy one===========================
            free_point, target = move_out_subnode(ax, trace, occupied, in_out)     # 移动枝干上的障碍物，下步将小车送到枝干上
            free_point, target = move_out_step4(ax, free_point, target, trace)      # 构建四个位置的循环，将 target 移出到 枝干上

            # 检查路径上是否存在障碍
            while True:
                points, points_obstacle = utils.check_obstacle(trace, occupied, target)
                if points_obstacle:     # 仍然存在障碍点
                    free_point, target = move_out_step5(ax, free_point, target,  trace)     # 构建六个位置的循环，在枝干上循环移动平板车
                else:
                    break

            # 按照通路移出小车  有待修改
            ump.move_pingbancar(points, ax, reset_car=True)                         # 在余下的路径上没有了障碍物，直接移出，同时将平板车复位
            # =============================== policy one===========================

            # move_to_out()
            # =============================== ===========================
            remove_pair = get_remove_car_trace(in_out, occupied, policy=cons.out_policy_two) # 移出小车的轨迹
            if len(remove_pair) == 2:
                park_position(ax, remove_pair, occupied, in_out=cons.OUT)
            elif len(remove_pair) == 4:
                park_position(ax, remove_pair, occupied, in_out=cons.OUT_POLICY_ONE)
            else:
                print('No Trace')
            utils_list.print_list(occupied)
        else:
            print("it must be 'in' or 'out'")



# line_pair = [[7, 17], [7.0, 27.0], [17.0, 27.0], [27.0, 27.0], [27.0, 17.0], [37.0, 17.0], [47.0, 17.0]]
# fig, ax = plt.subplots()
# for i in range(len(line_pair)):
#     if i==0:
#         pass
#     else:
#         ax.plot(line_pair[i-1], line_pair[i])
#
# plt.show()

# fig = plt.figure()
# ax = fig.subplots()
# plt.ion()
# # pair = [[1, 5], [5, 5], [10, 5], [10, 10]]
# pair = [[7, 17], [7.0, 27.0], [17.0, 27.0], [27.0, 27.0], [27.0, 17.0], [37.0, 17.0], [47.0, 17.0]]
# # 根据轨迹点得到轨迹图
# for i in range(len(pair)-1):
#     try:
#         ax.lines.remove(lines[0])
#         pass
#     except Exception:
#         pass
#       注意参数位置
#     lines = ax.plot([pair[i][0], pair[i+1][0]], [pair[i][1], pair[i+1][1]])
#     plt.pause(3)
# plt.show()